/**
 *
 * @file
 *		heap_dmk
 *
 * @brief
 *		Heap implementation by dm_karpov
 *
 *		There are two algorithms in my memory allocator: nextfit and tlsf with spinlock.
 *		The first is a bit slower than the second, but sometimes very slower.
 *		The first is N/2 for insert and O(N) for free, the second must be O(1).
 *		The advantage of nextfit based algorithm is that it hasn't got a big fragmentation 
 *		(as opposed to tlsf) because it starts each search where the last one left off,
 *		going to the beginning when the end of the list is reached.
 */
#include <Windows.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "heap_dmk.h"
#include "../../api/heap.h"
#include <vector>
#include <algorithm>
#include <iostream>
using namespace std;

#define Err printf
#undef _DEBUG
#ifdef _DEBUG
	//macro for debug output
	#define Dbg printf
#else//_DEBUG
	#define Dbg
#endif//_DEBUG
////////////////////////////////////////__TLSF__///////////////////////////////////

#define	ROUND_SIZE 4
#define	FL_INDEX_MAX 30
#define	SL_INDEX_MAX  32
#define	BLOCK_SIZE_MAX  (1 << 30)

typedef void* tlsf_pool;
typedef struct block_header_t
{
	struct block_header_t* prev_phys_block;
	unsigned int size;
	struct block_header_t* next_free;
	struct block_header_t* prev_free;
} block_header_t;
/*
 The two least significant bits of the size field store the block status:
 bit 0: whether block is busy or free
 bit 1: whether block is last of pool or not
*/
static const unsigned int block_header_free_bit = 1 << 0;
static const unsigned int block_header_prev_free_bit = 1 << 1;
static const size_t block_header_overhead = sizeof(unsigned int);
/* Used block data starts directy after the size field */
static const size_t block_start_offset = offsetof(block_header_t, next_free);
/* Minimum overage which will cause a free block split */
#define SPLIT_SIZE_MIN	(SL_INDEX_MAX + block_header_overhead)

typedef struct pool_t
{
	unsigned int valid;

	/* Bitmaps for free lists */
	unsigned int fl_bitmap;
	unsigned int sl_bitmap[FL_INDEX_MAX];

	/* Head of free lists */
	block_header_t* blocks[FL_INDEX_MAX][SL_INDEX_MAX];
} pool_t;

/* Wait for spin lock */
static int slwait (int *sl) {
    while (InterlockedCompareExchange ((long *) sl,  1, 0) != 0) 
	Sleep (0);
    return 0;
}
/* Release spin lock */
static int slrelease (int *sl) {
    InterlockedExchange ((long*)sl, 0);
    return 0;
}

static int pool_lock;

static const unsigned int tlsf_valid = 1189;
static int _ffs(unsigned int word)
{
    int bit = 0;

	if (word)
	{
		/* Clear out all but LSB bit */
		word &= ~word + 1;
		if (word & 0xffff0000) bit += 16;
		if (word & 0xff00ff00) bit += 8;
		if (word & 0xf0f0f0f0) bit += 4;
		if (word & 0xcccccccc) bit += 2;
		if (word & 0xaaaaaaaa) bit += 1;
		bit++;
	}

	return bit-1;
}

static int _fls(unsigned int word)
{
	int bit = 32;

	if (!word) bit -= 1;
	if (!(word & 0xffff0000)) { word <<= 16; bit -= 16; }
	if (!(word & 0xff000000)) { word <<= 8; bit -= 8; }
	if (!(word & 0xf0000000)) { word <<= 4; bit -= 4; }
	if (!(word & 0xc0000000)) { word <<= 2; bit -= 2; }
	if (!(word & 0x80000000)) { word <<= 1; bit -= 1; }

	return bit-1;
}

static size_t block_size(const block_header_t* block)
{
	return block->size & ~(block_header_free_bit | block_header_prev_free_bit);
}

static int block_is_last(const block_header_t* block)
{
	return 0 == block_size(block);
}

static int block_is_free(const block_header_t* block)
{
	return block->size & block_header_free_bit;
}

static void block_set_free(block_header_t* block)
{
	block->size |= block_header_free_bit;
}

static void block_set_used(block_header_t* block)
{
	block->size &= ~block_header_free_bit;
}

static int block_is_prev_free(const block_header_t* block)
{
	return block->size & block_header_prev_free_bit;
}

static void block_set_prev_free(block_header_t* block)
{
	block->size |= block_header_prev_free_bit;
}

static void block_set_prev_used(block_header_t* block)
{
	block->size &= ~block_header_prev_free_bit;
}

static block_header_t* next_physical_block(const block_header_t* block)
{
	block_header_t* next = 0;
	if (!block_is_last(block))
	{
		unsigned char* start = (unsigned char*)block;
		next = (block_header_t*)
			(start + block_size(block) + block_start_offset - block_header_overhead);
	}
	return next;
}

static void block_mark_as_free(block_header_t* block)
{
	block_header_t* next = next_physical_block(block);
	block_set_free(block);
	if (next)
	{
		next->prev_phys_block = block;
		block_set_prev_free(next);
	}
}

static void block_mark_as_used(block_header_t* block)
{
	block_header_t* next = next_physical_block(block);
	block_set_used(block);
	if (next)
	{
		block_set_prev_used(next);
	}
}

static block_header_t* block_from_ptr(void* ptr)
{
	return (block_header_t*)((unsigned char*)ptr - block_start_offset);
}

static void* ptr_from_block(block_header_t* block)
{
	return (void*)((unsigned char*)block + block_start_offset);
}


/*
	Adjust an allocation size to be rounded to word size
*/
static size_t adjust_request_size(size_t size)
{
	size_t adjust = (size + ROUND_SIZE - 1) & ~(ROUND_SIZE - 1);
	if (adjust < SL_INDEX_MAX)
	{
		adjust = SL_INDEX_MAX;
	}
	return adjust;
}

static void mapping_insert(size_t size, int* fli, int* sli)
{
	const int fl = _fls(size);
	const int sl = (size >> (fl - 5)) ^ (1 << 5);
	assert (size >= (1 << 5) && "input size is too small");
	*fli = fl;
	*sli = sl;
}

static void mapping_search(size_t size, int* fli, int* sli)
{
	const size_t round = (1 << (_fls(size) - 5)) - 1;
	mapping_insert(size + round, fli, sli);
}

static block_header_t* search_suitable_block(pool_t* pool, int* fli, int* sli)
{
	int fl = *fli;
	int sl = *sli;

	/*
	 First, search for a block in the list associated with
	 the given fl/sl index.
	*/
	unsigned int temp_bitmap = pool->sl_bitmap[fl] & (0xffffffff << sl);
	if (!temp_bitmap)
	{
		/* No block exists. Search in the next largest first-level list */
		temp_bitmap = pool->fl_bitmap & (0xffffffff << (fl + 1));
		if (!temp_bitmap)
		{
			/* No free blocks available, memory has been exhausted */
			return 0;
		}

		fl = _ffs(temp_bitmap);
		*fli = fl;
		temp_bitmap = pool->sl_bitmap[fl];
	}
	assert(temp_bitmap && "internal error - second level bitmap is null");
	sl = _ffs(temp_bitmap);
	*sli = sl;

	/* Return the first block in the free list */
	return pool->blocks[fl][sl];
}

/* Remove a free block from the free list*/
static void remove_free_block(pool_t* pool, block_header_t* block, int fl, int sl)
{
	block_header_t* prev = block->prev_free;
	block_header_t* next = block->next_free;
	block->prev_free = 0;
	block->next_free = 0;

	if (next)
	{
		next->prev_free = prev;
	}
	if (prev)
	{
		prev->next_free = next;
	}

	/* If this block is the head of the free list, set new head */
	if (pool->blocks[fl][sl] == block)
	{
		pool->blocks[fl][sl] = next;

		/* If the new head is null, clear the bitmap */
		if (!pool->blocks[fl][sl])
		{
			pool->sl_bitmap[fl] &= ~(1 << sl);

			/* If the second bitmap is now empty, clear the fl bitmap */
			if (!pool->sl_bitmap[fl])
			{
				pool->fl_bitmap &= ~(1 << fl);
			}
		}
	}
}

/* Insert a free block into the free block list */
static void insert_free_block(pool_t* pool, block_header_t* block, int fl, int sl)
{
	block_header_t* current = pool->blocks[fl][sl];
	block->prev_free = 0;
	block->next_free = current;
	if (current)
	{
		current->prev_free = block;
	}

	pool->blocks[fl][sl] = block;
	pool->fl_bitmap |= (1 << fl);
	pool->sl_bitmap[fl] |= (1 << sl);
}

/* Link a new block with its physical neighbor. */
static void link_physical_block(block_header_t* in)
{
	block_header_t* next = next_physical_block(in);
	next->prev_phys_block = in;
}

/* Split a free block into two. */
static block_header_t* split_block(pool_t* pool, block_header_t* block, size_t size)
{
	block_header_t* remaining = 
		(block_header_t*)((unsigned char*) block + size + block_header_overhead);
	const size_t remain_size = block_size(block) - (size + block_header_overhead);

	assert (0 == (remain_size % ROUND_SIZE));
	remaining->size = remain_size;

	block_mark_as_free(remaining);
	block_set_prev_free(remaining);

	remaining->prev_phys_block = block;
	remaining->next_free = 0;
	remaining->prev_free = 0;

	link_physical_block(remaining);

	block->size = size;
	block_set_free(block);

	return remaining;
}

/* Merge a just-freed block with an adjacent free block. */
static block_header_t* merge_left(pool_t* pool, block_header_t* block)
{
	if (block_is_prev_free(block))
	{
		int fl, sl;
		block_header_t* left = block->prev_phys_block;
		assert (!block_is_last(left) && "previous block can't be last!");
		mapping_insert(block_size(left), &fl, &sl);
		remove_free_block(pool, left, fl, sl);

		left->size += block_size(block) + block_header_overhead;
		link_physical_block(left);
		block = left;
	}

	return block;
}

/* Merge a just-freed block with an adjacent free block. */
static block_header_t* merge_right(pool_t* pool, block_header_t* block)
{
	block_header_t* right = next_physical_block(block);
	if (right && block_is_free(right))
	{
		int fl, sl;
		assert (!block_is_last(block) && "previous block can't be last!");
		mapping_insert(block_size(right), &fl, &sl);
		remove_free_block(pool, right, fl, sl);

		block->size += block_size(right) + block_header_overhead;
		link_physical_block(block);
	}

	return block;
}

tlsf_pool heap_dmk::tlsf_create(void* mem, size_t bytes)
{
	int sl, fl;
	pool_t* pool;
	block_header_t* block;
	block_header_t* next;

	if (bytes < sizeof(pool_t) + SPLIT_SIZE_MIN)
	{
		printf("Initialization Error : A pool must be at least %d bytes\n", sizeof(pool_t) + SPLIT_SIZE_MIN);
		exit(0);
	}

	pool = (pool_t*)mem;
	memset(pool,0 , sizeof(pool_t));

	block = (block_header_t*)((unsigned char*)mem + sizeof(pool_t) - block_header_overhead);
	block->next_free = 0;
	block->prev_free = 0;

	block->size = bytes - sizeof(pool_t) - block_header_overhead;
	block->size -= block_header_overhead; /* For the sentinel block */
	block->size -= (block->size % ROUND_SIZE); /* Round down */
	assert(0 == block->size % ROUND_SIZE);

	if (block->size > BLOCK_SIZE_MAX)
	{
		printf("This pool cannot store a %d-byte block. The maximum size is %d.\n",
			block_size(block), BLOCK_SIZE_MAX);
		exit(0);
	}

	block_set_free(block);
	block_set_prev_used(block);

	mapping_insert(block->size, &fl, &sl);
	insert_free_block(pool, block, fl, sl);

	/* Advance to the sentinel block, and set it properly */
	next = next_physical_block(block);
	next->size = 0;
	block_set_used(next);
	next->prev_phys_block = block;
	block_set_prev_free(next);

	/* Mark pool as valid and return */
	pool->valid = tlsf_valid;
	return (tlsf_pool) pool;
}

void heap_dmk::tlsf_destroy(tlsf_pool pool)
{
	pool_t* p = (pool_t*)pool;
	slwait(&pool_lock);
	assert(p->valid == tlsf_valid && "Not a TLSF pool");
	p->valid = ~tlsf_valid;
	slrelease(&pool_lock);
}

static void* tlsf_malloc(tlsf_pool tlsf, size_t size, unsigned int tag)
{
	int fl, sl;
	pool_t* pool = (pool_t*)tlsf;
	block_header_t* free_block;
	void* ptr = 0;

	size = adjust_request_size(size);
	slwait(&pool_lock);
	mapping_search(size, &fl, &sl);
	free_block = search_suitable_block(pool, &fl, &sl);
	if (free_block)
	{
		assert (block_size(free_block) >= size);
		remove_free_block(pool, free_block, fl, sl);
		if (block_size(free_block) - size >= SPLIT_SIZE_MIN)
		{
			block_header_t* remaining_block = split_block(pool, free_block, size);
			mapping_insert(block_size(remaining_block), &fl, &sl);
			insert_free_block(pool, remaining_block, fl, sl);
		}

		block_mark_as_used(free_block);
		ptr = ptr_from_block(free_block);
	}
	if (!ptr)
	{
		Err("\nAllocation failed : not enough memory\n");
		return NULL;
	}
	if(tag & heap876::alloc_debug)
	{
	
		for(int *bl=(int*)ptr, *bl_end=(int*)((size_t)ptr + free_block -> size); bl < bl_end; bl++)
		{
			if(*bl==POISON_ALLOC)
			{
				Err("alloc_debug : memory %p is not free, ptr=%p\n", bl, ptr);
				return(NULL);
			}
		}

		for(int *bl=(int*)ptr, *bl_end=(int*)((size_t)ptr + free_block -> size - block_start_offset); bl < bl_end; bl++)
		{
			*bl = POISON_ALLOC;
		}
	
	}
	slrelease(&pool_lock);
	return ptr;
}

static void tlsf_free(tlsf_pool tlsf, void* ptr, unsigned int tag)
{
	int fl, sl;
	pool_t* pool = (pool_t*)tlsf;
	if (!ptr)
	{
		printf("free() error: NULL pointer\n");
		exit(0);
	}
	slwait(&pool_lock);
	
	block_header_t* merged_block = block_from_ptr(ptr);

	merged_block = merge_left(pool, merged_block);
	merged_block = merge_right(pool, merged_block);
	block_mark_as_free(merged_block);

	mapping_insert(block_size(merged_block), &fl, &sl);
	insert_free_block(pool, merged_block, fl, sl);
	slrelease(&pool_lock);
}
///////////////////////////////////////___END TLSF__///////////////////////////////

//////////////////////////////////////__TLSF DEBUG FUNCTIONS__/////////////////////

#ifdef _DEBUG

static void block_info (block_header_t *bl){
  if (bl == 0) return;
  Dbg("Addres 0x%x  ", (unsigned int)bl);
  if (block_is_free(bl))
    Dbg ("\n Status FREE  ");
  else
    Dbg ("\n Status USED  ");
  Dbg ("Block size: %d bytes", bl->size << 2);
   if (!bl -> prev_phys_block)
    Dbg ("\n FIRST BLOCK");
  else 
    Dbg ("\n PREV. PHYS. BLOCK 0x%x", bl -> prev_phys_block);
  
  if (block_is_last(bl))
    Dbg (" LAST BLOCK");
  else
    Dbg (" NOT LAST BLOCK");

  if (block_is_free(bl)) {
    Dbg ("\n Prev Free 0x%x ", bl -> prev_free);
    Dbg (" Next Free 0x%x ", bl -> next_free);
    
  }
  Dbg ("\n\n");
}

void heap_dmk::all_blocks_info (char *block_ptr) {
  block_header_t *bl;
  pool_t *ptr_TLSF;
  ptr_TLSF = (pool_t *) block_ptr; 
  if (!ptr_TLSF || ptr_TLSF -> valid != tlsf_valid) {
    Dbg ("structure_status() error: TLSF structure is not initialized\n");
    Dbg (" Execute tlsf_create() before calling structure_status()\n");
      return;
  }
  Dbg ("\nTLSF structure address 0x%x ", ptr_TLSF);
  Dbg ("\n\nALL BLOCKS\n");
    for (int i = 0; i < 30; i++) {
         for (int j = 0; j < 30; j++) {
             bl = ptr_TLSF -> blocks[i][j];
             while (bl) {
				 Dbg("i = %d j = %d\n", i, j);
                 block_info(bl);
                 bl = bl -> prev_phys_block;
             }
         }
    }
}

void heap_dmk::free_blocks_info (char *block_ptr){
  int i, j;
  block_header_t *bl;
  pool_t *ptr_TLSF;
  ptr_TLSF = (pool_t *) block_ptr; 
  if (!ptr_TLSF || ptr_TLSF -> valid != tlsf_valid) {
    Dbg ("structure_status() error: TLSF structure is not initialized\n");
    Dbg (" Execute tlsf_create() before calling structure_status()\n");
      return;
  }

  Dbg ("\nTLSF structure address 0x", ptr_TLSF);
  Dbg ("\nFREE BLOCKS\n\n");
  for (i = 24; i >= 0; i--) {
    if (ptr_TLSF -> sl_bitmap[i] > 0)
      for (j = 31; j >= 0; j--) {
        if (ptr_TLSF -> blocks[i][j]) {
          bl = ptr_TLSF -> blocks[i][j];
          Dbg ("[%d] %d bytes -> Free blocks: 0x%x ", i + 5, (1 << (i + 5)), ptr_TLSF -> sl_bitmap[i] );
          Dbg ("\n");
          while (bl) {
			 Dbg ("First_Level [%d] Second Level [%d] -> %d bytes\n", i + 5, j, ((1 << (i + 5)) + ( ((1 << (i + 5)) / 32) * j)));
             block_info (bl);
             bl = bl-> next_free;
          }
        }
      }
  }
}


#endif

//////////////////////////////////////__END TLSF DEBUG FUNCTIONS__/////////////////

//////////////////////////////////////  NEXTFIT  //////////////////////////////////

void* heap_dmk::nextfit_alloc(size_t nbytes, unsigned int tag  )
{
	Header *p, *prevp;
	Header *morecore(size_t);
	size_t nunits;

	nunits = (nbytes + sizeof(Header) - 1)/sizeof(Header) + 1;
	if ((prevp = freep) == NULL) {			// list is not existed yet
		base.s.ptr = freep = prevp = &base;
		base.s.size = 0;
	}
	for (p = prevp->s.ptr; ;prevp = p, p = p->s.ptr) {
		if (p->s.size >= nunits) {		// enough size
			if (p->s.size == nunits)	// prompt size
				prevp->s.ptr = p->s.ptr;
			else {					// Cut the tail
				p->s.size -= nunits;
				p += p->s.size;
				p->s.size = nunits;
			}
			freep = prevp;
			not_free.push_back((void *)(p+1));
			return (void *)(p+1);
		}
		if (p == freep) 
			if ((p = heap_dmk::morecore(nunits)) == NULL) 
				return NULL;
					
	}
}

void heap_dmk::nextfit_free(void *ap)
{
	Header *bp, *p;
	if (!ap) exit(0);
/*	if (find(not_free.begin(), not_free.end(), ap) == not_free.end())
	{
		Err ("Deallocation of a pointer not alloceted: %p\n", ap);
		return;
	}*/
	bp = (Header *)ap - 1;     // Pointer to the header
	not_free.erase(remove(not_free.begin(), not_free.end(), ap), not_free.end());
	
	for (p = freep; !(bp > p && bp < p->s.ptr); p = p->s.ptr)
		if  (p >= p->s.ptr && (bp > p || bp < p->s.ptr))
			break;			// Releasing block in the end or in the beginning

		if (bp + bp->s.size == p->s.ptr) {	// To the upper neighbour
			bp->s.size += p->s.ptr->s.size;
			bp->s.ptr = p->s.ptr->s.ptr;
		} else 
			bp->s.ptr = p->s.ptr;
		if (p + p->s.size == bp) {		// To the bottom neighbour
			p->s.size += bp->s.size;
			bp->s.ptr = bp->s.ptr;
		} else 
			p->s.ptr = bp;
		freep = p;
}
////////////////////////////////////// END NEXTFIT  ////////////////////////////////////

void heap_dmk::free(void *ap)
{
#ifdef TLSF
	tlsf_free(pool, ap, flag);
#else
	nextfit_free(ap);
#endif
}

void* heap_dmk::alloc(size_t nbytes, unsigned int tag)
{
#ifdef TLSF
	return tlsf_malloc(pool, nbytes, flag);
#else
	return nextfit_alloc(nbytes);
#endif
	
}
void heap_dmk::free_all( unsigned int tag )
{
	if (heap876::alloc_tagged & flag) {
#ifdef TLSF
	 block_header_t *bl;
	 pool_t *ptr_TLSF;
     ptr_TLSF = (pool_t *) pool; 
	 for (int i = 0; i < 30; i++) {
         for (int j = 0; j < 30; j++) {
             bl = ptr_TLSF -> blocks[i][j];
             while (bl) {
				 if (!block_is_free(bl))
					 tlsf_free(ptr_TLSF, ptr_from_block(bl), flag);
                 bl = bl -> prev_phys_block;
             }
         }
    }
#else
	int size = not_free.size();
		for (int i = 0; i < size; i++)
			free(not_free[0]);
#endif
	}
}

size_t heap_dmk::get_occupancy() const
{
#ifdef TLSF
	 block_header_t *bl;
	 pool_t *ptr_TLSF;
	 int count = 0;
     ptr_TLSF = (pool_t *) pool; 
	 for (int i = 0; i < 30; i++) {
         for (int j = 0; j < 30; j++) {
             bl = ptr_TLSF -> blocks[i][j];
             while (bl) {
				 if (!block_is_free(bl))
					 count += bl -> size;
                 bl = bl -> prev_phys_block;
             }
         }
    }
	return(count);
#else
	return(heap_dmk::count);
#endif
}



